/* Functions for VGA modes */
#include <svga/vga_modes.h>
#include <svga/vga_regs.h>
#include <yatak/pokex.h>
#include <yatak/kheap.h>
#include <yatak/port.h>
#include <screen.h>
#include <string.h>

extern int graphical_mode;

unsigned long g_wd, g_ht, g_bpp;
unsigned g_planes, g_pixels;
unsigned fb_seg;
unsigned long* vga_mem = (unsigned long*)0xA0000;

unsigned char vga_320x200x256[] =
{
/* MISC */
	0x63,
/* SEQ */
	0x03, 0x01, 0x0F, 0x00, 0x0E,
/* CRTC */
	0x5F, 0x4F, 0x50, 0x82, 0x54, 0x80, 0xBF, 0x1F,
	0x00, 0x41, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x9C, 0x0E, 0x8F, 0x28,	0x40, 0x96, 0xB9, 0xA3,
	0xFF,
/* GC */
	0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x05, 0x0F,
	0xFF,
/* AC */
	0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07,
	0x08, 0x09, 0x0A, 0x0B, 0x0C, 0x0D, 0x0E, 0x0F,
	0x41, 0x00, 0x0F, 0x00,	0x00
};

uint32* double_buffer;
uint8* double_buffer_256;

static void setup_vesa_mode(unsigned short width, unsigned short height, unsigned short depth);

static void g_flip(uint32 *source, uint32 count) 
{
	memcpy((uint8*)vga_mem, (uint8*)source, count);
}

/*static void g_partial_flip(uint32 *source, uint32 x, uint32 y, uint32 w, uint32 h) 
{
	for(int i=0; i <= h; i++)
		memcpy((uint8*)vga_mem+(((y*g_wd)+x)+(i*g_wd)), (uint8*)source+(((y*g_wd)+x)+(i*g_wd)), w);
}*/

static unsigned get_fb_seg()
{
	unsigned seg;

	outportb(VGA_GC_INDEX, 6);
	seg = inportb(VGA_GC_DATA);
	seg >>= 2;
	seg &= 3;
	switch(seg)
	{
	case 0:
	case 1:
		seg = 0xA000;
		break;
	case 2:
		seg = 0xB000;
		vga_mem = (unsigned long*)0xB0000;
		break;
	case 3:
		seg = 0xB800;
		vga_mem = (unsigned long*)0xB8000;
		break;
	}
	return seg;
}

static void vsync()
{
	/* wait until any previous retrace has ended */
	do{} while (inportb(0x3DA) & 8);
	/* wait until a new retrace has just begun */
	do {
	} while (!(inportb(0x3DA) & 8));
}

void set_color(int color, int red, int green, int blue)
{
	vsync();
	outportb(0x3C8, color);
	outportb(0x3C9, red);
	outportb(0x3C9, green);
	outportb(0x3C9, blue);
}

void refresh_screen_32bpp()
{
	vsync();
	g_flip(double_buffer,(g_wd*g_ht*4));
}

/*void refresh_32bpp(uint32 x, uint32 y, uint32 w, uint32 h)
{
	vsync();
	g_partial_flip(double_buffer, x, y, w, h);
}*/

void refresh_screen_256()
{
	vsync();
	g_flip((uint32*)double_buffer_256,(g_wd*g_ht*(g_bpp/8)));
}

/*void refresh_256(uint32 x, uint32 y, uint32 w, uint32 h)
{
	vsync();
	g_partial_flip((uint32)double_buffer_256, x, y, w, h);
}*/

static void vpokeb(unsigned off, unsigned val)
{
	pokeb(fb_seg, off, val);
}

static unsigned vpeekl(unsigned off)
{
	return peekl(fb_seg, off);
}

void plot_pixel(unsigned x,unsigned y, unsigned c, unsigned width, uint32* buffer)
{
	buffer[(width * y + x)] = c;
}

void write_buffer(unsigned x, unsigned y, unsigned width, unsigned height, uint32* buffer)
{
	int j,h;
	for(j=y; j<y + height; j++)
		for(h=x; h<x + width; h++)
			g_write_pixel(h,j,buffer[((j-y)*width)+(h-x)]);
}

void put_buffer(unsigned x, unsigned y, unsigned width, unsigned height, uint32* buffer)
{
	int j,h;
	for(j=y; j<y + height; j++)
		for(h=x; h<x + width; h++)
			g_put_pixel(h,j,buffer[((j-y)*width)+(h-x)]);
}

void put_pixel_32bpp(unsigned x,unsigned y, unsigned c)
{
	vga_mem[(g_wd * y + x)] = c;
}

void write_pixel_32bpp(unsigned x,unsigned y, unsigned c)
{
	double_buffer[g_wd * y + x] = c;
}

uint32 read_pixel_32bpp(int x,int y)
{
	int offset = g_wd * y + x;
	return vga_mem[offset];
}

void put_pixel_256(unsigned x, unsigned y, unsigned c)
{
	if(x < 0 || x > (int)g_wd)
		return;
	if(y < 0 || y > (int)g_ht)
		return;
	vpokeb((g_wd * y + x), c);
}

void write_pixel_256(unsigned x, unsigned y, unsigned c)
{
	if(x < 0 || x > (int)g_wd) 
		return;
	if(y < 0 || y > (int)g_ht)
		return;
	double_buffer_256[g_wd*y+x] = c;
}


uint32 read_pixel_256(int x, int y)
{
	int offset = y * g_wd + x;
	return vpeekl(offset);
}

static void write_regs(unsigned char *regs)
{
	uint i;

	// write MISCELLANEOUS reg
	outportb(VGA_MISC_WRITE, *regs);
	regs++;
	// write SEQUENCER regs
	for(i = 0; i < VGA_NUM_SEQ_REGS; i++)
	{
		outportb(VGA_SEQ_INDEX, i);
		outportb(VGA_SEQ_DATA, *regs);
		regs++;
	}
	// unlock CRTC registers
	outportb(VGA_CRTC_INDEX, 0x03);
	outportb(VGA_CRTC_DATA, inportb(VGA_CRTC_DATA) | 0x80);
	outportb(VGA_CRTC_INDEX, 0x11);
	outportb(VGA_CRTC_DATA, inportb(VGA_CRTC_DATA) & ~0x80);
	// make sure they remain unlocked
	regs[0x03] |= 0x80;
	regs[0x11] &= ~0x80;
	// write CRTC regs
	for(i = 0; i < VGA_NUM_CRTC_REGS; i++)
	{
		outportb(VGA_CRTC_INDEX, i);
		outportb(VGA_CRTC_DATA, *regs);
		regs++;
	}
	// write GRAPHICS CONTROLLER regs
	for(i = 0; i < VGA_NUM_GC_REGS; i++)
	{
		outportb(VGA_GC_INDEX, i);
		outportb(VGA_GC_DATA, *regs);
		regs++;
	}
	// write ATTRIBUTE CONTROLLER regs
	for(i = 0; i < VGA_NUM_AC_REGS; i++)
	{
		(void)inportb(VGA_INSTAT_READ);
		outportb(VGA_AC_INDEX, i);
		outportb(VGA_AC_WRITE, *regs);
		regs++;
	}
	// lock 16-color palette and unblank display
	(void)inportb(VGA_INSTAT_READ);
	outportb(VGA_AC_INDEX, 0x20);
}

void set_vga_mode(int mode)
{
	switch(mode)
	{
	case VGA_MODE_13h:
		graphical_mode = 1;
		write_regs(VGA_320_200_256);
		g_wd = 320;
		g_ht = 200;
		g_bpp = 16;
		g_planes = 1;
		g_pixels = 8;
		g_put_pixel = put_pixel_256;
		g_write_pixel = write_pixel_256;
		g_read_pixel = read_pixel_256;
		refresh_screen = refresh_screen_256;
		//refresh = refresh_256;
		fb_seg = get_fb_seg();
		break;
	case VESA_MODE_809:
		graphical_mode = 2;
		g_wd = 600;
		g_ht = 480;
		g_bpp = 32;
		setup_vesa_mode(600, 480, 32);
		g_planes = 1;
		g_pixels = 8;
		g_put_pixel = put_pixel_32bpp;
		g_write_pixel = write_pixel_32bpp;
		g_read_pixel = read_pixel_32bpp;
		refresh_screen = refresh_screen_32bpp;
		//refresh = refresh_32bpp;
		break;
    case VESA_MODE_824:
		graphical_mode = 2;
		g_wd = 1024;
		g_ht = 768;
		g_bpp = 32;
		setup_vesa_mode(1024, 768, 32);
		g_planes = 1;
		g_pixels = 8;
		g_put_pixel = put_pixel_32bpp;
		g_write_pixel = write_pixel_32bpp;
		g_read_pixel = read_pixel_32bpp;
		refresh_screen = refresh_screen_32bpp;
		//refresh = refresh_32bpp;
		//g_displaybmp = displaybmp_256;
		break;
	default:
		//Unspecified mode supplied 
		break;
    }
   
	if(graphical_mode == 2)
	{
		//Create the double buffer
		double_buffer = (uint32*)kmalloc((g_wd*g_ht)*4);
		memset((uint8*)double_buffer,0,g_wd*g_ht*(g_bpp/8));
	}
	else
	{
		double_buffer_256 = (uint8*)kmalloc(g_wd*g_ht*(g_bpp/8));
		memset(double_buffer_256,0,g_wd*g_ht*(g_bpp/8));
	}
	// Clear to black
	draw_rect(0,0,g_wd,g_ht,0x00000000); 
	return;
}

static void setup_vesa_mode(unsigned short width, unsigned short height, unsigned short depth)
{
	//soon to be some fancy hardwear detection and
	//therefore driver loading.
	//for now we will assume bochs/qemu
	vga_mem = (unsigned long*)0;
     
	bochs_vbe_set_mode(width,height,depth);
     
	while(vga_mem == 0) __asm__ __volatile__ ("nop");
	memset((uint8*)vga_mem,0,g_wd*g_ht*(g_bpp/8));
}

void setcolour(int x, int y, int r, int g, int b)
{
	uint32 colour1, colour2, colour3=0;

	if (r > 255)
		r = 255;
	if (r < 0)
		r = 0;
	if (g > 255)
		g = 255;
	if (g < 0)
		g = 0;
	if (b > 255)
		b = 255;
	if (b < 0)
		b = 0;
   
	// Shift the RGB bits and assign them to 
	// a 32 bit word.
	colour1 = r << 16;
	colour2 = g << 8;
	colour3 = b;
	//Now they're in the correct position, combine
	//them using OR's
	colour1 |= colour2;
	colour1 |= colour3;
	kprintf(" 0x%x (%x %x %x) ",colour1,r,g,b);
	return;
}